/**
 * @file tf.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief 坐标转换节点
 * @version 0.1
 * @date 2020-07-21
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"

#include "map_driver.h"
#include "serial_driver.h"
#include "storm_car/car_status.h"

#include <tf/transform_broadcaster.h>
#include <ros/ros.h>

double begin_plan_path_x, begin_plan_path_y;


void SerialCallback(const storm_car::car_status::ConstPtr& msg);

int main(int argc, char** argv){
  ros::init(argc, argv, "TF");

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe("serial_receive_data", 10, &SerialCallback);

  ros::Rate loop_rate(10);

  std::vector<std::string> v_temp;
  std::string temp;
  std::ifstream fin(FILEPATH);

  tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::Quaternion q;

  if(!fin)
  {
    ROS_INFO("OPEN FILE FAILED");
    return false;
  }

  if (getline(fin, temp))
  {
    SplitString(temp, v_temp, ",");
    map::LonLat2UTM(StringToDouble(v_temp[2].c_str()),StringToDouble(v_temp[1].c_str()), begin_plan_path_x, begin_plan_path_y);
  }


  while(ros::ok())
  {
    transform.setOrigin( tf::Vector3(-0.7, 0.0, 0.0) );
    q.setRPY(PI, 0, PI/2);
    transform.setRotation(q);

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/car", "/gps"));

    transform.setOrigin( tf::Vector3(2, 0.0, 0.0) );
    q.setRPY(0, 0, 0);
    transform.setRotation(q);

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/car", "/lidar"));
    loop_rate.sleep();
    ros::spinOnce();
  }
  
  return 0;
};

void SerialCallback(const storm_car::car_status::ConstPtr& msg)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  double x,y;
  tf::Quaternion q;

  map::LonLat2UTM(msg->longitude, msg->lattitude, x, y);
  
  transform.setOrigin( tf::Vector3(x-begin_plan_path_x, y-begin_plan_path_y, 0.0) );
  q.setRPY(0, 0, 0);
  // q.setRPY(PI, 0, PI/2);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/car", "/plan"));

}